format long
P = ...
[0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,8.84326172,0.00000000,0.06149289,0.00884766,0.29558411,0.00884765,-0.00000299,0.00000000,0.00000000,0.00000003,0.00000088,0.00000003
0.00000000,0.00000000,0.00000000,0.00000000,8.84333992,0.08269805,-0.29558671,0.00884774,-0.00000013,0.00000000,-0.00000299,0.00000000,-0.00000088,0.00000003,0.00000000
0.00000000,0.00000000,0.00000000,0.06149289,0.08269805,0.00221004,-0.00270295,0.00213837,0.00006153,0.00000000,0.00000000,-0.00000299,-0.00000001,0.00000001,0.00000000
0.00000000,0.00000000,0.00000000,0.00884766,-0.29558671,-0.00270295,0.01089785,-0.00000000,0.00000886,0.00000000,0.00000000,0.00000000,-0.00000296,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.29558411,0.00884774,0.00213837,-0.00000000,0.01089776,0.00029573,0.00000000,0.00000000,0.00000000,-0.00000000,-0.00000296,0.00000000
0.00000000,0.00000000,0.00000000,0.00884765,-0.00000013,0.00006153,0.00000886,0.00029573,0.00101781,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,-0.00000299
0.00000000,0.00000000,0.00000000,-0.00000299,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000000,-0.00000299,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,-0.00000299,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000003,-0.00000088,-0.00000001,-0.00000296,-0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,0.00000000,-0.00000000
0.00000000,0.00000000,0.00000000,0.00000088,0.00000003,0.00000001,0.00000000,-0.00000296,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,-0.00000000
0.00000000,0.00000000,0.00000000,0.00000003,0.00000000,0.00000000,0.00000000,0.00000000,-0.00000299,0.00000000,0.00000000,0.00000000,-0.00000000,-0.00000000,0.00000200];
H = ...
[0.00000000,0.00000000,1393.76855469,649.42199707,19.43912506,-0.33527148,-4.14246464,-338.85244751,-191.34754944,0.00000000,0.00000000,0.00000000,0.00000000,-65.00000000,0.00000000
0.00000000,0.00000000,-1913.47534180,19.44050217,-649.13513184,19.30179977,-333.23730469,-0.09878445,-139.37689209,0.00000000,0.00000000,0.00000000,-65.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000];
R = ...
[0.50000000,0.00000000,0.00000000
0.00000000,0.50000000,0.00000000
0.00000000,0.00000000,0.00000000];


Z = H*P*transpose(H) + R
Z_inv = inv(Z(1:2,1:2))
K = P*transpose(H)*[Z_inv, zeros(2,1); zeros(1,3)]